# -*- coding:utf-8 -*-
# @Time : 2021/2/26 下午9:08
# @Author :  rebeater
# @File : ImuConverter.py
# @Project:  NavigationTool_qt
# @Function: IMU format convert

import numpy as np
import struct
import pandas as pd


class OutputOption:
    def __init__(self):
        self.ImuFilePath = None
        self.ImuType = None
        self.OutputMode = None
        self.OutputAcceUnit = None
        self.OutputGyroUnit = None
        self.OutputFrame = None
        self.OutputFilePath = None
        self.Rate = 0


class ImuDataConvert:
    def __init__(self):
        self.data = None
        self.exception = None
        self.file_type = 0

    def read_imuraw(self, file_path):
        # imuraw  rad/s 1
        try:
            data = np.fromfile(file_path, "double")
            self.data = data.reshape([-1, 7])
            self.file_type = 0
        except Exception as err:
            self.exception = err

    def read_imd(self, file_path):
        try:
            data = np.fromfile(file_path, "double")
            self.data = data.reshape([-1, 7])
            self.file_type = 2
        except Exception as err:
            self.exception = err

    def read_imutxt(self, file_path):
        try:
            self.data = np.loadtxt(file_path)
            self.file_type = 1
        except Exception as err:
            self.exception = err

    def to_imd(self, file_path, rate: int, g: np.float, convert_frame=False):
        ts = 1.0 / rate
        if self.file_type == 0:
            self.data[:, 1:4] *= ts
            self.data[:, 4:7] *= (g * ts)
        if convert_frame:
            self.convert_frame()
        with open(file_path, 'wb') as f:
            for line in self.data:
                for data in line:
                    b = struct.pack("<d", data)
                    f.write(b)

    def to_imutxt(self, file_path, rate: int, g: np.float, convert_frame=False):
        ts = 1.0 / rate
        if self.file_type == 0:
            self.data[:, 1:4] *= ts
            self.data[:, 4:7] *= (g * ts)
        if convert_frame:
            self.convert_frame()
        with open(file_path, 'w') as f:
            for d in self.data:
                f.write("%.4f %.7f %.7f %.7f %.7f %.7f %.7f\r\n" % (d[0], d[1], d[2], d[3], d[4], d[5], d[6]))

    def convert_frame(self):
        self.data[:, 1], self.data[:, 2] = self.data[:, 2], self.data[:, 1]
        self.data[:, 3] *= -1
        self.data[:, 4], self.data[:, 5] = self.data[:, 5], self.data[:, 4]
        self.data[:, 6] *= -1


class ImuDataProcess:
    def __init__(self, file_name, file_type, convert_frame, imurate=100):
        """
        :param file_name:
        :param file_type:  0: bin,  1: ascii
        """
        self.file_name = file_name
        self.data = None
        self.e = None
        self.time_diff = None
        if file_type == 0:
            try:
                data = np.fromfile(file_name, "double")
                if data.shape[0] % 7 != 0:
                    data = data[:-(data.shape[0] % 7)]
                self.data = data.reshape([-1, 7])
                self.time_diff = np.diff(self.data[:, 0])  # 时间差，检查数据完整性
                if convert_frame:
                    self.data = self.__convert_frame(self.data)
            except Exception as err:
                self.e = err
        else:
            try:
                self.data = np.loadtxt(self.file_name, dtype=np.double)
                self.time_diff = np.diff(self.data[:, 0])  # 时间差，检查数据完整性
                if convert_frame:
                    self.data = self.__convert_frame(self.data)
            except Exception as err:
                self.e = err

    # def convert(self,opt:OutputOption):

    def convert2bin(self, bin_file_name):
        with open(bin_file_name, 'wb') as f:
            for line in self.data:
                for data in line:
                    b = struct.pack("<d", data)
                    f.write(b)

    def convert2txt(self, text_file_name):
        df = pd.DataFrame(self.data)
        df.to_csv(text_file_name, sep=" ", header=False, index=False, float_format="%.8f")

    # def convert_frame(self, converted_file_name, file_type=1):
    #     new_data = self.__convert_frame(self.data)
    #     if file_type == 1:
    #         df = pd.DataFrame(new_data)
    #         df.to_csv(converted_file_name, sep=" ")
    #     if file_type == 0:
    #         with open(converted_file_name, 'wb') as f:
    #             for line in new_data:
    #                 for data in line:
    #                     b = struct.pack("<d", data)
    #                     f.write(b)

    def __convert_frame(self, data):
        new_data = np.zeros_like(data, "double")
        new_data[:, 0] = data[:, 0]
        new_data[:, 1] = data[:, 2]
        new_data[:, 2] = data[:, 1]
        new_data[:, 3] = -data[:, 3]
        new_data[:, 4] = data[:, 5]
        new_data[:, 5] = data[:, 4]
        new_data[:, 6] = -data[:, 6]
        return new_data
